/*
 * motor.c
 *
 *  Created on: 2024/5/24
 *      Author: RRRRRRRun
 */
#include "zf_common_headfile.h"

uint8 motor_stop = 0; // 停车！

int8 duty_sl = 0;     //侧左输出值
int8 duty_sr = 0;     //侧右输出值

int8 duty_rl = 0;     //后左输出值
int8 duty_rr = 0;     //后右输出值


uint16 duty_susl = 0; //左悬浮输出
uint16 duty_susr = 0; //右悬浮输出
//-------------------------------------------------------------------------------------------------------------------
// 函数简介    初始化悬浮电机
// 参数说明     无
// 返回参数     无
// 使用示例     Suspension_motor_init();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void Suspension_motor_init()
{
    pwm_init(Suspemotor_l, 50, 0);
    pwm_init(Suspemotor_r, 50, 0);
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     输出悬浮电机
// 参数说明     duty
// 返回参数     无
// 使用示例     Suspension_motor_set(650, 700);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void Suspension_motor_set(uint32 duty1, uint32 duty2)
{
    if(motor_stop)duty1 = duty2 = 0;
    pwm_set_duty(Suspemotor_l, clip(duty1, 500, 1000 - 100));
    pwm_set_duty(Suspemotor_r, clip(duty2, 500, 1000 - 100));
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     初始化左侧系统动力及控制电机
// 参数说明     无
// 返回参数     无
// 使用示例     Left_motor_init();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void Left_motor_init()
{

    gpio_init(DIR_RL, GPO, GPIO_HIGH, GPO_PUSH_PULL);                            // GPIO 初始化为输出 默认上拉输出高
    pwm_init(PWM_RL, 17000, 0);                                                  // PWM 通道初始化频率 17KHz 占空比初始为 0

    gpio_init(DIR_SL, GPO, GPIO_HIGH, GPO_PUSH_PULL);                            // GPIO 初始化为输出 默认上拉输出高
    pwm_init(PWM_SL, 17000, 0);
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     初始化右侧系统动力及控制电机
// 参数说明     无
// 返回参数     无
// 使用示例     Power_motor_init();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void Right_motor_init()
{
    gpio_init(DIR_RR, GPO, GPIO_HIGH, GPO_PUSH_PULL);                                // GPIO 初始化为输出 默认上拉输出高
    pwm_init(PWM_RR, 17000, 0);                                                  // PWM 通道初始化频率 17KHz 占空比初始为 0

    gpio_init(DIR_SR, GPO, GPIO_HIGH, GPO_PUSH_PULL);                                // GPIO 初始化为输出 默认上拉输出高
    pwm_init(PWM_SR, 17000, 0);                                                  // PWM 通道初始化频率 17KHz 占空比初始为 0
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     左侧系统动力及控制电机输出
// 参数说明     无
// 返回参数     无
// 使用示例     Left_motor_set();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void Left_motor_set()
{
    int8 duty_r = duty_rl;
    int8 duty_s = duty_sl;
    //限幅
    if(duty_r >= MAX_DUTY_POWERMOTOR)
        duty_r = MAX_DUTY_POWERMOTOR;
    if(duty_r <= MIN_DUTY_POWERMOTOR)
        duty_r = MIN_DUTY_POWERMOTOR;
    if(duty_s >= MAX_DUTY_POWERMOTOR)
        duty_s = MAX_DUTY_POWERMOTOR;
    if(duty_s <= MIN_DUTY_POWERMOTOR)
        duty_s = MIN_DUTY_POWERMOTOR;
    //计算输出

    if(motor_stop)duty_s = duty_r = 0;

    if(duty_r>0)
    {
        gpio_set_level(DIR_RL, GPIO_HIGH);                                          // DIR输出低电平
        pwm_set_duty(PWM_RL, duty_r * (PWM_DUTY_MAX / 100));                       // 计算占空比
    }
    else
    {
        gpio_set_level(DIR_RL, GPIO_LOW);                                         // DIR输出高电平
        pwm_set_duty(PWM_RL, ( -duty_r) * (PWM_DUTY_MAX / 100));                   // 计算占空比
    }
    if(duty_s>0)
    {
        gpio_set_level(DIR_SL, GPIO_HIGH);                                         // DIR输出高电平
        pwm_set_duty(PWM_SL, duty_s * (PWM_DUTY_MAX / 100));                       // 计算占空比
    }
    else
    {
        gpio_set_level(DIR_SL, GPIO_LOW);                                          // DIR输出低电平
        pwm_set_duty(PWM_SL, ( -duty_s) * (PWM_DUTY_MAX / 100));                   // 计算占空比
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     右侧系统动力及控制电机输出
// 参数说明     无
// 返回参数     无
// 使用示例     Right_motor_set();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void Right_motor_set()
{
    //限幅
    int8 duty_r = duty_rr;
    int8 duty_s = duty_sr;
    if(duty_r >= MAX_DUTY_POWERMOTOR)
        duty_r = MAX_DUTY_POWERMOTOR;
    if(duty_r <= MIN_DUTY_POWERMOTOR)
        duty_r = MIN_DUTY_POWERMOTOR;
    if(duty_s >= MAX_DUTY_POWERMOTOR)
        duty_s = MAX_DUTY_POWERMOTOR;
    if(duty_s <= MIN_DUTY_POWERMOTOR)
        duty_s = MIN_DUTY_POWERMOTOR;
    //计算输出

    if(motor_stop)duty_s = duty_r = 0;

    if(duty_r>0)
    {
        gpio_set_level(DIR_RR, GPIO_LOW);                                         // DIR输出高电平
        pwm_set_duty(PWM_RR, duty_r * (PWM_DUTY_MAX / 100));                       // 计算占空比
    }
    else
    {
        gpio_set_level(DIR_RR, GPIO_HIGH);                                          // DIR输出低电平
        pwm_set_duty(PWM_RR, ( -duty_r) * (PWM_DUTY_MAX / 100));                   // 计算占空比
    }
    if(duty_s>0)
    {
        gpio_set_level(DIR_SR, GPIO_HIGH);                                         // DIR输出高电平
        pwm_set_duty(PWM_SR, duty_s * (PWM_DUTY_MAX / 100));                       // 计算占空比
    }
    else
    {
        gpio_set_level(DIR_SR, GPIO_LOW);                                          // DIR输出低电平
        pwm_set_duty(PWM_SR, ( -duty_s) * (PWM_DUTY_MAX / 100));                   // 计算占空比
    }
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     清零电机状态
// 参数说明     无
// 返回参数     无
// 使用示例     Motor_duty_clear();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void Motor_duty_clear()
{
    duty_rl = 0;
    duty_sl = 0;

    duty_rr = 0;
    duty_sr = 0;

    duty_susl = 0;
    duty_susr = 0;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介    满负荷悬浮电机状态
// 参数说明     无
// 返回参数     无
// 使用示例     Motor_duty_game_um();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void Motor_duty_game_um()
{
    duty_susl = 950;
    duty_susr = 950;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介    左电机调试状态
// 参数说明     无
// 返回参数     无
// 使用示例     Motor_duty_debug_lm();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void Motor_duty_debug_lm()
{
    duty_rl = 30;
    duty_sl = 30;

    duty_rr = 0;
    duty_sr = 0;

    duty_susl = 0;
    duty_susr = 0;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介    右电机调试状态
// 参数说明     无
// 返回参数     无
// 使用示例     Motor_duty_debug_rm();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void Motor_duty_debug_rm()
{
    duty_rl = 0;
    duty_sl = 0;

    duty_rr = 30;
    duty_sr = 30;

    duty_susl = 0;
    duty_susr = 0;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介    无刷电机调试状态
// 参数说明     无
// 返回参数     无
// 使用示例     Motor_duty_debug_rm();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void Motor_duty_debug_um()
{
    duty_rl = 0;
    duty_sl = 0;

    duty_rr = 0;
    duty_sr = 0;

    duty_susl = 600;
    duty_susr = 600;
}
